#include "motor_control.h"

static u08 pwma;
static u08 pwmb;
static int dira;
static int dirb;


void setChannelADir(int dir){
	TCCR1A &= ~((1<<COM1A1) | (1<<COM1A0));
	dira = dir ;
	if(dira){
		TCCR1A |= (1<<COM1A1);          // Clear OC1A on Compare Match when up-counting.
                                  // Set OC1A on Compare Match when down-counting.
		PORTB &= ~(1 << PB0) ;
	}else{
		TCCR1A |= (1<<COM1A1) | (1<<COM1A0);
		PORTB |= (1 << PB0) ;
	}

}

void setChannelBDir(int dir){
	TCCR1A &= ~((1<<COM1B1) | (1<<COM1B0));
	dirb = dir ;
	if(dirb){
		TCCR1A |= (1<<COM1B1);          // Clear OC1B on Compare Match when up-counting.
                                  // Set OC1B on Compare Match when down-counting.
		
		PORTB &= ~(1 << PB3) ;
	}else{
		TCCR1A |= (1<<COM1B1) | (1<<COM1B0);
		PORTB |= (1 << PB3) ;
	}
	
}

void setupPWM(void){
  DDRB |= (1<<PB0);               // make PD6 an output
  DDRB |= (1<<PB3);               // make PD5 an output
  DDRB |= (1<<PB1);               // make OC1A an output
  DDRB |= (1<<PB2);               // make OC1B an output


  TCCR1B = 0;                     // stop timer 1
  TCCR1A = (1<<WGM12)|(1<<WGM10); // Fast PWM, 8-bit mode 5
  TCCR1A |= (1<<COM1A1);          // Clear OC1A on Compare Match when up-counting.
                                  // Set OC1A on Compare Match when down-counting.
  TCCR1A |= (1<<COM1B1);          // Clear OC1B on Compare Match when up-counting.
                                  // Set OC1B on Compare Match when down-counting.
  OCR1A = 0x00;                   // duty cycle
  OCR1B = 0x00;                   // duty cycle
  TCCR1B |= (1<<CS11);            // no prescaling timer on


  PORTB &= ~(1 << PB0) ;
  PORTB &= ~(1 << PB3) ;
  pwma = 0 ;
  pwmb = 0 ;
  dira = 1 ;
  dirb = 1 ;
}

void setPWMA(u08 value){
	pwma = value ;
	OCR1A = pwma ;
}

void setPWMB(u08 value){
	pwmb = value ;
	OCR1B = pwmb ;
}

void setSpeedA(u08 value){
	setPWMA(value);
}

void setSpeedB(u08 value){
	setPWMB(value);
}

void setChannelABrake(void){
  TCCR1A &= ~((1<<COM1A1)  | (1<<COM1A0));  // normal port mode
  PORTB |= (1<<PB0);             // '1' on both ports
  PORTB |= (1<<PB1); 
}

void setChannelBBrake(void){
  TCCR1A &= ~((1<<COM1B1)  | (1<<COM1B0));  // normal port mode
  PORTB |= (1<<PB3);             // '1' on both ports
  PORTB |= (1<<PB2); 
}

void setChannelAFree(void){
  DDRB &= ~(1<<PB0);               // make PD5 an input
  DDRB &= ~(1<<PB1);               // make OC1A an input
}

void setChannelBFree(void){
  DDRB &= ~(1<<PB3);               // make PD5 an input
  DDRB &= ~(1<<PB2);               // make OC1A an input
}

void setChannelARun(void){
  DDRB |= (1<<PB0);               // make PD5 an output
  DDRB |= (1<<PB1);               // make OC1A an output 
  setChannelADir(dira);                               
}

void setChannelBRun(void){
  DDRB |= (1<<PB3);               // make PD5 an output
  DDRB |= (1<<PB2);               // make OC1A an output
  setChannelBDir(dirb);
  
}
